#include "augmavconn/message_fifo.h"
//#include "augmavconn.h"

//void Write_Buffer(uint8_t ID, mavlink_message_t buffer);

/******************************************************/
Message_FIFO_WR::Message_FIFO_WR()//构造函数，初始化各属性变量
{
    priv=&fifotype;//FIFO指针指向FIFO
    priv->w_index=BUFSIZE;//FIFO写指针为BUFSIZE，FIFO为空
    empty=1;//FIFO空标志为1
    full=0;
    data_count=0;
}

void Message_FIFO_WR::WriteFIFO(mavlink_message_t data)//写入一个mavlink_message_t数据
{//printf("wFIFO\n");
    empty=0;//当有数据写入，则FIFO非空

    if (priv->w_index == 0)//当FIFO写指针为0时，FIFO已写满，需要，依次向前移动FIFO数据，新数据将写入buffer[0]
    {
        full=1;//FIFO满
        move();//FIFO数据移动
        priv->w_index++;//写指针移动
    }
    priv->w_index--;//写指针先移动，在写入数据
    copy_mavmessage(data,&(priv->buffer[priv->w_index]));
    
     //printf("wr:inmsgid=%d,msgid=%d\n",data.msgid,priv->buffer[priv->w_index].msgid);
}
void Message_FIFO_WR::ReadFIFO(mavlink_message_t *dataout)//读出一个mavlink_message_t数据
{
    full=0;//当有数据读取时，FIFO非满
    if(priv->w_index==BUFSIZE)//当写指针为BUFSIZE时，FIFO为空
    {
      empty=1;
    }
    if (!empty)//当FIFO非空时，从buffer[BUFSIZE-1]读取数据
    {
        try
        {
            copy_mavmessage(priv->buffer[BUFSIZE-1],dataout);
        }
        catch(char *ex)
        {
            ROS_ERROR("%s",ex);
        }
        move();
        priv->w_index++;
    }
}

bool Message_FIFO_WR::Isempty()//判断FIFO是否为空
{
    if(priv->w_index==BUFSIZE)
    {
        empty=1;
    }
    return empty;
}

bool Message_FIFO_WR::Isfull()//判断FIFO是否为满
{
    if(priv->w_index==0)
    {
        full=1;
    }
    return full;
}

void Message_FIFO_WR::move()//移动FIFO内部数据
{
    mavlink_message_t temp;
    if((priv->w_index)<(BUFSIZE-1))//当指针大于(BUFSIZE-1)时，FIFO中只有一个数据，不需要移动。否则移动数据
    {
        for (int i=(BUFSIZE-2);i>=(int)(priv->w_index);i--)//从第二个数据开始向前一个数据存储区移动，直到移动完写指针指向的数据区
        {
            copy_mavmessage(priv->buffer[i],&temp);
            copy_mavmessage(temp,&(priv->buffer[i+1]));
        }
    }
}

unsigned int Message_FIFO_WR::get_index()//获得写指针
{
    return priv->w_index;
}



/******************************************************************************/
Message_FIFO_WR_T::Message_FIFO_WR_T()//构造函数，初始化各属性变量
{
    Timer_count=1;
    mes_tcount=0;
}


void Message_FIFO_WR_T::Initial(int time, int id)
{
    if(time!=0)
    {
        time_num=time/100;
    }
    else
    {
        time_num=1;
    }
    ID=id;
    Interval=time;
}

int Message_FIFO_WR_T::getInterval()
{
    return Interval;
}
int Message_FIFO_WR_T::getID()
{
    return ID;
}

uint8_t Message_FIFO_WR_T::getTimer_count()
{
    return Timer_count;
}

void Message_FIFO_WR_T::Timer_count_add()
{
    if(Timer_count==time_num)
    {
        Timer_count=1;
    }
    else
    {
       Timer_count++;
    }
}

uint8_t Message_FIFO_WR_T::gettime_num()
{
    return time_num;
}

void Message_FIFO_WR_T::cls_Tcount()
{
    Timer_count=1;
}

void Message_FIFO_WR_T::set()
{
   Timer->AddHander(this,&Message_FIFO_WR_T::send_data);

   // connect( Timer,SIGNAL(timeout()), this, SLOT(send_data()) );
}//定时器信号与槽函数连接

void Message_FIFO_WR_T::connect()
{
   Timer->add_mTmessage(this);
}

void Message_FIFO_WR_T::get_outmessage(mavlink_message_t* pmavlink_message)
{
   copy_mavmessage(outmessage,pmavlink_message);
   //printf("get_message:FIFOID=%d,outmsgid=%d,msgid=%d\n",ID,outmessage.msgid,pmavlink_message->msgid);
}

void Message_FIFO_WR_T::set_outmessage(uint8_t flag,uint8_t dex)
{
    if(flag==1)
    {
        copy_mavmessage(priv->buffer[dex],&outmessage);
        //printf("set_message:FIFOID=%d,msgid=%d,outmsgid=%d\n",ID,priv->buffer[dex].msgid,outmessage.msgid);
        read_flag=true;
    }
    else if(flag==0)
    {
        bool flag=Isempty();
        if(!flag)//若FIFO非空，则可以读取FIFO数据
        {
            ReadFIFO(&outmessage);
            read_flag=true;
        }
        else
        {
            read_flag=false;
        }
    }
}

void Message_FIFO_WR_T::send_data()//定时器的槽函数
{
    uint8_t result=Timer_count%time_num;//计算FIFO的定时
    if(result==0)
   {
        //printf("ID=%d",ID);
        Timer_count=1;

        if(read_flag)//若FIFO非空，则可以读取FIFO数据
        {
            get_outmessage(&(pBuffer->buffer));//读取FIFO数据
            pBuffer->length=sizeof(pBuffer->buffer);//记录数据长度
            Write_Buffer(ID,pBuffer->buffer);
        }
        data_count++;

    }
    else
    {
       Timer_count++;
    }

}

/******************************************************************************/

void Message_FIFO_WR_T_Aperiodic::WriteFIFO(mavlink_message_t data)
{
    Message_FIFO_WR::WriteFIFO(data);
    ApFIFO_Isempty(); //emit Write_to_ApFIFO(); //发送信号
}

void Message_FIFO_WR_T_Aperiodic::ApFIFO_Isempty()
{
    Timer_count=0;
    set_outmessage(0,0);
    Message_FIFO_WR_T::send_data();

}
 Message_FIFO_WR_T_Aperiodic::Message_FIFO_WR_T_Aperiodic()
{
    pBuffer=NULL;
}

/******************************************************************************/
//void(*funct)(uint8_t*,int);
augmavconn::AugMavConnInterface* augptr=NULL;
void
SetDataHandler(augmavconn::AugMavConnInterface*  aptr)
{
    augptr=aptr;
}

void Write_Buffer(uint8_t ID,mavlink_message_t buffer)//uint8_t *buffer
{
    augptr->real_send_message(buffer,ID);
}
/******************************************************************************/
void copy_mavmessage(mavlink_message_t datain,mavlink_message_t *dataout)
{
    dataout->checksum=datain.checksum;
    dataout->magic  = datain.magic;
    dataout->len  = datain.len;
    dataout->seq  = datain.seq;
    dataout->sysid  = datain.sysid;
    dataout->compid  = datain.compid;
    dataout->msgid  = datain.msgid;
    for (int i =0;i<datain.len;i++ ){
        *((uint8_t* )(dataout->payload64)+i)=*((uint8_t* )(datain.payload64)+i);
    }
}
